#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET    -1
#define POT_PIN       34    // 电位器连接引脚
#define THROTTLE_PIN  15    // 油门开关连接引脚
#define DIRECTION_PIN 13    // 方向开关连接引脚

// 电机引脚定义
#define LEFT_RPWM  14    // 左电机前进
#define LEFT_LPWM  27    // 左电机后退
#define RIGHT_RPWM 26    // 右电机前进
#define RIGHT_LPWM 12    // 右电机后退

// PWM参数
#define PWM_FREQ 5000    // PWM频率
#define PWM_RES  8       // PWM分辨率(8位=0-255)
#define MAX_FORWARD_RPM 150  // 最大正转速度
#define MAX_REVERSE_RPM 10   // 最大反转速度

// 死区阈值 (避免电机在低转速时抖动)
#define DEADZONE_THRESHOLD 0.1

Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

void setup() {
  Serial.begin(115200);
  
  // 初始化引脚
  pinMode(THROTTLE_PIN, INPUT_PULLUP);  // 油门开关（内部上拉）
  pinMode(DIRECTION_PIN, INPUT_PULLUP); // 方向开关（内部上拉）
  
  // 初始化电机控制引脚
  ledcSetup(0, PWM_FREQ, PWM_RES);  // 左电机前进通道
  ledcAttachPin(LEFT_RPWM, 0);
  ledcSetup(1, PWM_FREQ, PWM_RES);  // 左电机后退通道
  ledcAttachPin(LEFT_LPWM, 1);
  ledcSetup(2, PWM_FREQ, PWM_RES);  // 右电机前进通道
  ledcAttachPin(RIGHT_RPWM, 2);
  ledcSetup(3, PWM_FREQ, PWM_RES);  // 右电机后退通道
  ledcAttachPin(RIGHT_LPWM, 3);
  
  // 初始化OLED
  Wire.begin(4, 5); // SDA=4, SCL=5
  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("SSD1306 allocation failed"));
    while(1);
  }
  
  // 显示初始信息
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(WHITE);
  display.setCursor(0,0);
  display.println("EV Control System");
  display.println("Initializing...");
  display.display();
  delay(1000);
  
  // 初始状态显示
  updateDisplay(false, 0.0, 0.0, "STOP", "RELEASED", "FORWARD");
}

void loop() {
  // 读取油门状态（LOW表示按下）
  bool throttlePressed = !digitalRead(THROTTLE_PIN);
  
  // 读取方向状态（LOW表示后退）
  bool directionForward = digitalRead(DIRECTION_PIN); // HIGH=前进, LOW=后退
  
  // 读取电位器值并计算电压
  int potValue = analogRead(POT_PIN);
  float voltage = potValue * (3.3 / 4095.0);  // ESP32 ADC分辨率12位(0-4095)
  
  // 计算转速（电压映射到0-1范围）
  float speedRatio = constrain(voltage / 3.3, 0.0, 1.0);
  
  // 计算电机控制值
  float rpm = 0;
  String motorState = "STOP";
  String directionState = directionForward ? "FORWARD" : "REVERSE";
  
  if (throttlePressed) {
    if (speedRatio > DEADZONE_THRESHOLD) {
      if (directionForward) {
        // 前进模式
        rpm = mapFloat(speedRatio, DEADZONE_THRESHOLD, 1.0, 0, MAX_FORWARD_RPM);
        motorState = "RUNNING";
        
        // 设置电机前进
        int pwmValue = mapFloat(speedRatio, DEADZONE_THRESHOLD, 1.0, 0, 255);
        ledcWrite(0, pwmValue);  // 左电机前进
        ledcWrite(1, 0);         // 左电机后退
        ledcWrite(2, pwmValue);  // 右电机前进
        ledcWrite(3, 0);         // 右电机后退
      } else {
        // 后退模式
        rpm = mapFloat(speedRatio, DEADZONE_THRESHOLD, 1.0, 0, MAX_REVERSE_RPM);
        motorState = "RUNNING";
        
        // 设置电机后退
        int pwmValue = mapFloat(speedRatio, DEADZONE_THRESHOLD, 1.0, 0, 255);
        ledcWrite(0, 0);         // 左电机前进
        ledcWrite(1, pwmValue);  // 左电机后退
        ledcWrite(2, 0);         // 右电机前进
        ledcWrite(3, pwmValue);  // 右电机后退
      }
    } else {
      // 在死区内，停止电机
      motorState = "STOP";
      stopMotors();
    }
  } else {
    // 油门未按下，停止电机
    motorState = "STOP";
    stopMotors();
  }
  
  // 更新OLED显示
  updateDisplay(throttlePressed, voltage, rpm, motorState, throttlePressed ? "PRESSED" : "RELEASED", directionState);
  
  delay(50);  // 控制刷新率
}

// 停止所有电机
void stopMotors() {
  ledcWrite(0, 0);
  ledcWrite(1, 0);
  ledcWrite(2, 0);
  ledcWrite(3, 0);
}

// 浮点数映射函数
float mapFloat(float x, float in_min, float in_max, float out_min, float out_max) {
  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}

// 更新OLED显示
void updateDisplay(bool throttlePressed, float voltage, float rpm, String motorState, String throttleState, String directionState) {
  display.clearDisplay();
  
  // 标题栏
  display.setTextSize(1);
  display.setCursor(0, 0);
  display.print("EV Control System");
  
  // 分割线
  display.drawLine(0, 10, 128, 10, WHITE);
  
  // 显示方向状态
  display.setCursor(0, 15);
  display.print("Direction: ");
  display.setTextColor(directionState == "FORWARD" ? WHITE : BLACK, directionState == "FORWARD" ? BLACK : WHITE);
  display.print(directionState);
  display.setTextColor(WHITE);
  
  // 显示油门状态
  display.setCursor(0, 30);
  display.print("Throttle: ");
  display.setTextColor(throttlePressed ? WHITE : BLACK, throttlePressed ? BLACK : WHITE);
  display.print(throttleState);
  display.setTextColor(WHITE);
  
  // 显示电机状态和RPM
  display.setCursor(0, 45);
  display.print("Motor: ");
  display.print(motorState);
  
  display.setCursor(75, 45);
  display.print("RPM: ");
  display.print(rpm, 1);
  
  // 绘制进度条
  float maxRpm = directionState == "FORWARD" ? MAX_FORWARD_RPM : MAX_REVERSE_RPM;
  float progress = (maxRpm > 0) ? constrain(rpm / maxRpm, 0.0, 1.0) : 0.0;
  
  drawProgressBar(10, 55, 108, 8, progress, directionState);
  
  // 添加进度条标签
  display.setTextSize(1);
  display.setCursor(0, 55);
  display.print("0");
  display.setCursor(50, 55);
  display.print((int)(progress * 50) * 2); // 显示0-100%
  display.print("%");
  display.setCursor(110, 55);
  display.print(maxRpm, 0);
  
  display.display();
}

// 进度条绘制函数
void drawProgressBar(int x, int y, int width, int height, float progress, String state) {
  // 绘制边框
  display.drawRect(x, y, width, height, WHITE);
  
  // 计算填充宽度
  int fillWidth = (int)(progress * (width - 2));
  fillWidth = constrain(fillWidth, 0, width - 2);
  
  // 绘制填充部分
  if (state == "REVERSE") {
    // 反转：从右向左填充（红色表示）
    display.fillRect(x + width - fillWidth - 1, y + 1, fillWidth, height - 2, WHITE);
  } else {
    // 正转：从左向右填充（绿色表示）
    display.fillRect(x + 1, y + 1, fillWidth, height - 2, WHITE);
  }
}